#include <windows.h>
#include <sys/timeb.h>


#include <iostream>
#include <string.h>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/ximgproc.hpp>
#include <opencv2/optflow.hpp>
#include "joint_bilateralfilter.h"
#include "humanseg_base.h"


using namespace std;
using namespace cv;

static void gray8_sobel(const unsigned char* src_plane, int pitch_src, int width, int height, unsigned char* dst_plane, int pitch_dst);


uint32_t time_ms()
{
    struct timeb t;
    
    ftime(&t);
    
    return  (uint32_t)(t.time * 1000 + t.millitm);
}

class HumanSegPrivate
{
	public:
		HumanSegPrivate(int model_width, int model_height, int enable_debug):model_width_(model_width), model_height_(model_height), enable_debug_(enable_debug){
			int aligned_w, aligned_h;

			aligned_w = (model_width + 0xf) & (~0xf);
			aligned_h = (model_height + 0xf) & (~0xf);

			buf_alpha_last_ = new unsigned char  [aligned_w * aligned_h];
			buf_alpha_cur_ = new unsigned char  [aligned_w * aligned_h];
			buf_alpha_tmp_ = new unsigned char  [aligned_w * aligned_h];

			buf_snap_last_ = new unsigned char  [aligned_w * aligned_h];
			buf_snap_cur_ = new unsigned char  [aligned_w * aligned_h];
			buf_snap_tmp_ = new unsigned char  [aligned_w * aligned_h];

			buf_flow_fw_ = new float  [aligned_w * aligned_h * 2];
			buf_flow_bw_ = new float  [aligned_w * aligned_h * 2];

			memset(buf_alpha_last_, 0, aligned_w * aligned_h);
			memset(buf_alpha_cur_, 0, aligned_w * aligned_h);
			memset(buf_alpha_tmp_, 0, aligned_w * aligned_h);
			memset(buf_flow_fw_, 0, sizeof(float) * 2 * aligned_h * aligned_w);
			memset(buf_flow_bw_, 0, sizeof(float) * 2 * aligned_h * aligned_w);

			snap_pitch_ = aligned_w;
			alpha_pitch_ = model_width_; 
            dis_opt_flow_ = cv::DISOpticalFlow::create(cv::DISOpticalFlow::PRESET_ULTRAFAST);
		}

		~HumanSegPrivate()
		{
			delete buf_alpha_last_;
			delete buf_alpha_cur_;
			delete buf_alpha_tmp_;

			delete buf_snap_tmp_;
			delete buf_snap_last_;
			delete buf_snap_cur_;
			delete buf_flow_fw_;
			delete buf_flow_bw_;

		};
	public:
		unsigned char  * buf_alpha_last_;
		unsigned char  * buf_alpha_cur_;
		unsigned char  * buf_alpha_tmp_;

		unsigned char  * buf_snap_last_;
		unsigned char  * buf_snap_cur_;
		unsigned char  * buf_snap_tmp_;

		float 			*buf_flow_fw_;
		float 			*buf_flow_bw_;

		int snap_pitch_;
		int alpha_pitch_;
		int model_width_;
		int model_height_;
		int enable_debug_;

    private:
        Ptr<DenseOpticalFlow> dis_opt_flow_;
        
	public:
		void preprocess_input(cv::Mat & in_rgb, cv::Mat & out_rgb)
		{
			cv::GaussianBlur(in_rgb, out_rgb, Size(3, 3), 3, 3);
		}

		void smooth_with_last_alpha_optic_flow(cv::Mat &fg_gray, unsigned char *alpha_seg, int pitch_seg);
        void smooth_with_last_alpha(cv::Mat &fg_gray, unsigned char *alpha_cur, int pitch_cur);
		void fixed_alpha_model_size(cv::Mat &alpha_in, cv::Mat &alpha_out);
		void fixed_alpha_with_fg(cv::Mat &fg_gray, cv::Mat &alpha_in, cv::Mat &alpha_out);
		void output_alpha(cv::Mat &fg_gray, unsigned char *alpha_cur, int pitch_cur, int out_width, int out_height, unsigned char *alpha_out, int pitch_out, int is_video);
	
};


void HumanSegPrivate::output_alpha(cv::Mat &fg_gray,unsigned char *alpha_seg, int pitch_seg, int out_width, int out_height, unsigned char *alpha_out, int pitch_out, int is_video)
{
	Mat mat_alpha_out(out_height, out_width, CV_8UC1, alpha_out, pitch_out);


	if (is_video){
        uint32_t t0, t1, t2, t3;
		// output alpha to buf_alpha_cur_

        t0 = time_ms();
		smooth_with_last_alpha(fg_gray, alpha_seg, pitch_seg);
        t1 = time_ms();

		// threshold and keep the max contours 
		Mat alpha0(model_height_, model_width_, CV_8UC1);
		Mat alpha_cur(model_height_, model_width_, CV_8UC1, buf_alpha_cur_, alpha_pitch_);
		fixed_alpha_model_size(alpha_cur,  alpha0);
        t2 = time_ms();

		// ouput alpha with the same size of fg_gray 
		fixed_alpha_with_fg(fg_gray, alpha0, mat_alpha_out);

		t3 = time_ms();

        
        //printf("output alpha delay %dms , details = %dms, %dms, %dms\n", t3 - t0, t1 - t0, t2 - t1, t3 - t2);

	}else {
		Mat alpha0(model_height_, model_width_, CV_8UC1, alpha_seg, pitch_seg);
		Mat alpha1;

		if (enable_debug_){
			cv::imshow("orig_mask", alpha0);
			cv::waitKey(0);
		}

		fixed_alpha_model_size(alpha0, alpha1);
		if (enable_debug_){
			cv::imshow("fixed_mask0", alpha1);
			cv::waitKey(0);
		}
	
		fixed_alpha_with_fg(fg_gray, alpha1, mat_alpha_out);

	}
}

void HumanSegPrivate::smooth_with_last_alpha_optic_flow(cv::Mat &fg_gray, unsigned char *alpha_seg, int pitch_seg)
{
	// alpha -> last_alpha
	std::swap(buf_alpha_last_, buf_alpha_cur_);
	std::swap(buf_snap_last_, buf_snap_cur_);

	cv::Mat snap_cur(model_height_, model_width_, CV_8UC1, buf_snap_cur_, snap_pitch_);
	cv::Mat snap_last(model_height_, model_width_, CV_8UC1, buf_snap_last_, snap_pitch_);
	cv::Mat snap_tmp(model_height_, model_width_, CV_8UC1, buf_snap_tmp_, snap_pitch_);
	
	//1  save snap , blur 
	cv::resize(fg_gray, snap_tmp,  Size(model_width_, model_height_), 0, 0, cv::INTER_AREA);
	cv::GaussianBlur(snap_tmp, snap_cur, Size(3, 3), 3, 3);

    cv::Mat prev_cfd(model_height_, model_width_, CV_8UC1, buf_alpha_last_);
    cv::Mat track_cfd = cv::Mat::zeros(model_height_, model_width_, CV_8UC1); 

    cv::Mat weights = cv::Mat::ones(model_height_, model_width_, CV_32FC1) * 0.3f; 
    cv::Mat mat_is_track =  cv::Mat::zeros(model_height_, model_width_, CV_8UC1);
    cv::Mat flow_fw(model_height_, model_width_, CV_32FC2, buf_flow_fw_, sizeof(float) * snap_pitch_ * 2);
    cv::Mat flow_bw(model_height_, model_width_, CV_32FC2, buf_flow_bw_, sizeof(float) * snap_pitch_ * 2); 


	//calc flow opt flow crash in some reso, just workaround 
	{
		cv::Mat pre_gray(model_height_, snap_pitch_, CV_8UC1, buf_snap_last_, snap_pitch_);
		cv::Mat cur_gray(model_height_, snap_pitch_, CV_8UC1, buf_snap_cur_, snap_pitch_);
		cv::Mat flow_fw1(model_height_, snap_pitch_, CV_32FC2, buf_flow_fw_, sizeof(float) * snap_pitch_ * 2);
		cv::Mat flow_bw1(model_height_, snap_pitch_, CV_32FC2, buf_flow_bw_, sizeof(float) * snap_pitch_ * 2); 
		dis_opt_flow_->calc(pre_gray, cur_gray, flow_fw1);
		dis_opt_flow_->calc(cur_gray, pre_gray, flow_bw1);  

	}

    int col, row;
  
    int check_thres = 8;

    for(row = 0 ; row < flow_fw.rows ; ++row){
        Vec2f *ptr_flow_fw = flow_fw.ptr<Vec2f>(row);

        for(col = 0 ; col < flow_fw.cols; ++col){
            float flow_fw_x = ptr_flow_fw[col][0];
            float flow_fw_y = ptr_flow_fw[col][1];
            int is_track = 1 ;
            int not_flow = 0;
            int cur_x, cur_y;
            Vec2f flow_bw_cur;
            float flow_bw_cur_x, flow_bw_cur_y;

            cur_x = (int32_t)(col + flow_fw_x);
            cur_y = (int32_t)(row + flow_fw_y);
            
            if (cur_x < 0 || cur_x >= flow_fw.cols || cur_y < 0 || cur_y >= flow_fw.rows)
                is_track = 0;

            if (is_track){
                flow_bw_cur = flow_bw.at<Vec2f>(cur_y, cur_x);
                flow_bw_cur_x = flow_bw_cur[0];
                flow_bw_cur_y = flow_bw_cur[1];
                
                if ( (flow_fw_x + flow_bw_cur_x) * (flow_fw_x + flow_bw_cur_x) + (flow_fw_y + flow_bw_cur_y) * (flow_fw_y + flow_bw_cur_y) >= check_thres)
                    is_track = 0;
            }else {
                flow_bw_cur =   flow_bw.at<Vec2f>(row, col);
                flow_bw_cur_x = flow_bw_cur[0];
                flow_bw_cur_y = flow_bw_cur[1];
            }
            
            if (is_track){
                track_cfd.at<uint8_t>(cur_y, cur_x) = prev_cfd.at<uint8_t>(row, col);
            }

            if (fabs(flow_fw_x) < 0.5 && fabs(flow_fw_y) < 0.5 && fabs(flow_bw_cur_x) < 0.5 && fabs(flow_bw_cur_y) < 0.5)
                not_flow = 1;

            if (not_flow){
                weights.at<float>(cur_y, cur_x) = 0.05f;
            }

            if (is_track)
                mat_is_track.at<uint8_t>(cur_y, cur_x) = 1;
        }

    }

    
    cv::Mat alpha(model_height_, model_width_, CV_8UC1, alpha_seg, pitch_seg);
    cv::Mat alpha_cur(model_height_, model_width_, CV_8UC1, buf_alpha_cur_, alpha_pitch_);

    alpha.copyTo(alpha_cur);


    
    for(row = 0 ; row < flow_fw.rows ; ++row){
        float *p_weight = weights.ptr<float>(row);
        uint8_t *p_seg_cfd = alpha_cur.ptr<uint8_t>(row);
        uint8_t  *p_track_cfd  = track_cfd.ptr<uint8_t>(row);
        uint8_t *p_is_track = mat_is_track.ptr<uint8_t>(row);
        
        for(col = 0 ; col < flow_fw.cols; ++col){
            if (p_is_track[col]){
                float weight;
                int seg_val;
            
                weight = p_weight[col];
                if ((p_seg_cfd[col] > (int)(255 * 0.9) || p_seg_cfd[col] < (int)(255 * 0.1))){
                    if (weight < 0.1){//index_less01
                        seg_val = (int)(0.3f * p_seg_cfd[col] + 0.7f * p_track_cfd[col]);
                        p_seg_cfd[col] = CLIP_UIN8(seg_val);
                    }else {//index_large
                        seg_val = (int)(0.4f * p_seg_cfd[col] + 0.6f * p_track_cfd[col]);
                        p_seg_cfd[col] = CLIP_UIN8(seg_val);
                    }
                }else {
                    seg_val = (int)(p_seg_cfd[col] + weight *(p_track_cfd[col]  - p_seg_cfd[col]));
                    p_seg_cfd[col] = CLIP_UIN8(seg_val);
                }
            }
        }
    }
    
}

void HumanSegPrivate::smooth_with_last_alpha(cv::Mat &fg_gray, unsigned char *alpha_seg, int pitch_seg) 
{
	std::swap(buf_snap_last_, buf_snap_cur_);

	cv::Mat snap_cur(model_height_, model_width_, CV_8UC1, buf_snap_cur_, snap_pitch_);
	cv::Mat snap_last(model_height_, model_width_, CV_8UC1, buf_snap_last_, snap_pitch_);
	cv::Mat snap_tmp(model_height_, model_width_, CV_8UC1, buf_snap_tmp_, snap_pitch_);

	//1  save snap , blur , and calc motion feature with last frame 
	cv::resize(fg_gray, snap_tmp,  Size(model_width_, model_height_), 0, 0, cv::INTER_AREA);

	cv::GaussianBlur(snap_tmp, snap_cur, Size(3, 3), 3, 3);
	cv::absdiff(snap_cur, snap_last, snap_tmp);

    //2  smooth update alpha 

    int row, col;
    unsigned char  *p_cur, *p_last, *p_diff;

    
	for(row = 0 ; row < model_height_ ; ++row){
	    p_cur = alpha_seg + row * pitch_seg;
	    p_last = buf_alpha_cur_ + row * model_width_; 
	    p_diff = snap_tmp.ptr<unsigned char>(row); 
	    
	    for(col = 0; col < model_width_ ; ++col){
	        uint16_t weight = p_diff[col] > 10 ? 8 : 2;

	        //printf("w = %d, dog = %d, row = %d, col = %d\n", w, p_diff[col], col, row);
	        //p_last[col] = (unsigned char )((weight * p_cur[col] + (8 - weight) * p_last[col]) / 8);
	        p_last[col] = (unsigned char )(p_last[col] +  ( (weight * (p_cur[col] - p_last[col])) >> 3 ));
	    }
	}
	

}
void HumanSegPrivate::fixed_alpha_model_size(cv::Mat &alpha_in, cv::Mat &alpha_out)
{
    cv::Mat alpha0;
    
    auto kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(3, 3), cv::Point(-1, -1));
	if (0){
		cv::Mat alpha1;
		// 255 * 0.05 ~ 13
		// https://github.com/yucornetto/MGMatting/blob/main/code-base/utils/util.py#L209
		alpha_in.copyTo(alpha_out);
		cv::threshold(alpha_in, alpha1, 13, 255, cv::THRESH_BINARY);
		// morphologyEx with OPEN operation to remove noise first.
		cv::morphologyEx(alpha1, alpha0, cv::MORPH_OPEN, kernel);
	}else {
		cv::threshold(alpha_in, alpha_out, 100, 0, cv::THRESH_TOZERO);
		// morphologyEx with OPEN operation to remove noise first.
		cv::morphologyEx(alpha_out, alpha0, cv::MORPH_OPEN, kernel);
	}

    // Computationally connected domain
    cv::Mat labels = cv::Mat::zeros(alpha0.size(), CV_32S);
    cv::Mat stats, centroids;
    int num_labels = cv::connectedComponentsWithStats(alpha0, labels, stats, centroids, 8, 4);
    if (num_labels <= 1) return ; // no object, skip.

    
    // find max connected area, 0 is background
    int max_connected_id = 1; // 1,2,...
    int max_connected_area = stats.at<int>(max_connected_id, cv::CC_STAT_AREA);
    for (int i = 1; i < num_labels; ++i)
    {
      int tmp_connected_area = stats.at<int>(i, cv::CC_STAT_AREA);
      if (tmp_connected_area > max_connected_area)
      {
        max_connected_area = tmp_connected_area;
        max_connected_id = i;
      }
    }
    int h = alpha0.rows;
    int w = alpha0.cols;

    // remove small connected area.
    for (int i = 0; i < h; ++i)
    {
      int *label_row_ptr = labels.ptr<int>(i);
      unsigned char  *alpha_row_ptr = alpha_out.ptr<unsigned char >(i);
      for (int j = 0; j < w; ++j)
      {
        if (label_row_ptr[j] != max_connected_id){
 		   //printf("fixed %d,%d from %d -> 0\n", j, i, alpha_row_ptr[j]);
		   alpha_row_ptr[j] = 0;
        }else {
        }
      }
    }

	
} 

void HumanSegPrivate::fixed_alpha_with_fg(cv::Mat &fg_gray, cv::Mat &alpha_in, cv::Mat &alpha_out)
{
	cv::Mat mat_border(fg_gray.rows, fg_gray.cols, CV_8UC1);
	unsigned char *buf_alpha_resized, *buf_border;

	cv::Mat fg_gray_blur;
	cv::Mat mat_alpha_resized;
	cv::Mat mat_alpha_guided(fg_gray.rows, fg_gray.cols, CV_8UC1);

	if (enable_debug_){
		printf("fixed_alpha_with_fg 1, fg_gray sizeo = %dx%d\n", fg_gray.cols, fg_gray.rows);
	}
		
	cv::resize(alpha_in, mat_alpha_resized, Size(fg_gray.cols, fg_gray.rows));

	if (enable_debug_){
		cv::imshow("alpha resized", mat_alpha_resized);
		cv::waitKey(0);
	}

	cv::GaussianBlur(fg_gray, fg_gray_blur, Size(3, 3), 3, 3);

	buf_alpha_resized = (unsigned char *)mat_alpha_resized.data;
	buf_border		  = (unsigned char *)mat_border.data;

	gray8_sobel(buf_alpha_resized, (int)mat_alpha_resized.step[0], fg_gray.cols, fg_gray.rows, buf_border, (int)mat_border.step[0]);

    if (1){
        cv_jointBilateralFilter_8u(fg_gray_blur, mat_alpha_resized, alpha_out, -1, 5, 4, mat_border); 
        //cv::GaussianBlur(mat_alpha_guided, alpha_out, Size(3, 3), 3, 3);

    }else {
        cv_jointBilateralFilter_8u(fg_gray_blur, mat_alpha_resized, alpha_out, -1, 5, 4, mat_border);

    }


}

HumanSegBase::HumanSegBase(const char *model_name, int model_width, int model_height, int is_video, int enable_debug):model_name_(model_name), 
							 model_width_(model_width & (~1u)), model_height_(model_height & (~1u)), is_video_(is_video), enable_debug_(enable_debug)
{

	priv_data_ = (void *) new HumanSegPrivate(model_width_, model_height_, enable_debug);
}

 HumanSegBase::~HumanSegBase()
{

	//printf("HumanSegBase: destructor !!!");
	delete (HumanSegPrivate *)priv_data_;
}




int HumanSegBase::detect_i420(int width, int height, unsigned char **src_plane, int *src_pitch, unsigned char *alpha, int alpha_pitch)
{
	// 1 - make yuv continuous 

	width &= ~1u;
	height &= ~1u;


    //on continuous buf 
    if (width == src_pitch[0] && width / 2 == src_pitch[1] && width / 2 == src_pitch[2] && 
        src_plane[1] == src_plane[0] + width * height &&  src_plane[2] == src_plane[1] + (width * height / 4)
    ){
        Mat dst_YUV(height * 3 / 2, width, CV_8UC1, src_plane[0]);
        
        // 2 - YUV2RGB 
        cv::Mat rgb_in;  
        cv::cvtColor(dst_YUV, rgb_in, cv::COLOR_YUV2RGB_I420); 
        
        unsigned char *buf_rgb;
        
        buf_rgb = (unsigned char *)rgb_in.data;
        
        // 3 - detect rgb 
        if (enable_debug_){
            cv::imshow("test2", rgb_in);
            cv::waitKey(0);
        }
        return  detect_rgb24(width, height, buf_rgb, (int)rgb_in.step[0],  alpha,  alpha_pitch);

    }else {
        Mat src_Y(height, width, CV_8UC1, src_plane[0], src_pitch[0]);
        Mat src_U(height / 2, width / 2, CV_8UC1, src_plane[1], src_pitch[1]);
        Mat src_V(height / 2, width / 2, CV_8UC1, src_plane[2], src_pitch[2]);
        
        
        Mat dst_YUV(height * 3 / 2, width, CV_8UC1);
        unsigned char *buf_yuv;
        
        buf_yuv = (unsigned char *)dst_YUV.data;
        
        
        Mat dst_Y(height, width, CV_8UC1, buf_yuv);
        Mat dst_U(height / 2, width / 2, CV_8UC1, buf_yuv + height * width);
        Mat dst_V(height / 2, width / 2, CV_8UC1, buf_yuv + height * width * 5 / 4);
        
        
        src_Y.copyTo(dst_Y);
        src_U.copyTo(dst_U);
        src_V.copyTo(dst_V);
        
        
        
        // 2 - YUV2RGB 
        cv::Mat rgb_in;  
        cv::cvtColor(dst_YUV, rgb_in, cv::COLOR_YUV2RGB_I420); 
        
        
        unsigned char *buf_rgb;
        
        buf_rgb = (unsigned char *)rgb_in.data;
        
        // 3 - detect rgb 
        
        return  detect_rgb24(width, height, buf_rgb, (int)rgb_in.step[0],  alpha,  alpha_pitch);

    }
	
}

int HumanSegBase::detect_rgb24(int width, int height, unsigned char *buf_rgb, int src_pitch, unsigned char *alpha, int alpha_pitch)
{
	cv::Mat mat_in(height, width, CV_8UC3, buf_rgb, src_pitch);
	cv::Mat   mat_det_in;
	width &= ~1u;
	height &= ~1u;
    uint32_t t0, t1, t2, t3, t4;
	
	if (enable_debug_){
		cv::imshow("orig", mat_in);
		cv::waitKey(0);
	}


	HumanSegPrivate *priv = (HumanSegPrivate *)priv_data_;

	if (enable_debug_)
		printf("detect_rgb24, width = %d, height = %d, src_pitch = %d, alpha_pitch = %d\n", width, height, src_pitch, alpha_pitch);

    t0 = time_ms();
	if (width != model_width_ || height != model_height_){
		// 1 resize to detected size 
		cv::Mat mat_resized;
		
		cv::resize(mat_in, mat_resized,  Size(model_width_, model_height_), 0, 0, cv::INTER_AREA);
		// 2 preprocess 

		priv->preprocess_input(mat_resized, mat_det_in);
		
	}else {

		priv->preprocess_input(mat_in, mat_det_in);
	}

    t1 = time_ms();

	// 3 - do detect 
	unsigned char *buf_det;

	buf_det = (unsigned char *)mat_det_in.data;

	detect_rgb24_do(buf_det, (int)mat_det_in.step[0], priv->buf_alpha_tmp_,  priv->alpha_pitch_);


    t2 = time_ms();

	// 4 output alpha 

	cv::Mat mat_gray;

	cv::cvtColor(mat_in, mat_gray, COLOR_RGB2GRAY); 

    t3 = time_ms();


	priv->output_alpha(mat_gray, priv->buf_alpha_tmp_,  priv->alpha_pitch_, width, height, alpha, alpha_pitch,  is_video_);

    t4 = time_ms();


    if (0)
    printf("delay = %dms, details = %dms, %dms, %dms, %dms\n", t4 - t0, t1 - t0, t2 - t1, t3 - t2, t4 - t3);
    
	return 0;
	
}

/*

DX:
    -1  0 1 
    -2  0 2
    -1  0 1
    
DY:
    1  2  1 
    0  0  0 
   -1 -2 -1 
 
*/

static void gray8_sobel(const unsigned char* src_plane, int pitch_src, int width, int height, unsigned char* dst_plane, int pitch_dst)
{

	const unsigned char * p_src;
    unsigned char * p_dst;
	signed short   dx, dy, s;
	int skip0, skip1;
	int w, h;
    uint8_t v00, v01, v02;
    uint8_t v10, v11, v12;
    uint8_t v20, v21, v22;    

	skip0 = pitch_src - width + 2;
	skip1 = pitch_dst - width + 2;

	p_src = src_plane + pitch_src + 1;
	p_dst = dst_plane + pitch_dst + 1;

	for (h = 1; h < height - 1; h++) {
		for (w = 1; w < width - 1; w++) {
            v00 = p_src[-pitch_src - 1]; v01 = p_src[-pitch_src] ; v02 = p_src[-pitch_src + 1];
            v10 = p_src[           - 1]; v11 = p_src[         0] ; v12 = p_src[             1];
            v20 = p_src[ pitch_src - 1]; v21 = p_src[ pitch_src] ; v22 = p_src[pitch_src  + 1];

            dx = (int16_t)abs(-v00 + v01 - v10 - v10 + v12 + v12 - v20 + v22);
            dy = (int16_t)abs(v00  + v01 + v01 + v02 - v20 - v21 - v21 - v22);

            s  = dx + dy;
            *p_dst = CLIP_UIN8(s);

			p_src++;
			p_dst++;
		}
		p_src += skip0;
		p_dst += skip1;
	}

}

int HumanSegBase::bg_replace_i420(int width, int height, unsigned char **fg_plane, int *fg_pitch, 
			unsigned char **bg_plane, int *bg_pitch, unsigned char **mix_plane, int *mix_pitch,
			unsigned char *alpha, int alpha_pitch)
{


	int copy_w;
	int copy_h;
	int row, col;
	unsigned char *p_fg, *p_bg, *p_alpha, *p_dst;
    //plane 0
    copy_w = width;
    copy_h = height;
    
    for (row = 0; row < copy_h; ++row) {
        p_fg = fg_plane[0] + row * fg_pitch[0];
        p_bg = bg_plane[0] + row * bg_pitch[0];            
        p_alpha = alpha + row * alpha_pitch;  
        p_dst = mix_plane[0] + row * mix_pitch[0]; 
    
        for (col = 0; col < copy_w; ++col) {
            //*p_dst = ((bg) * (256 - alpha) + alpha * fg) >> 8;
            p_dst[col] = p_bg[col] + ((p_alpha[col] * ((signed short)p_fg[col] - p_bg[col])) >> 8);
        }
    }

    // plane 1
    copy_w /= 2;
    copy_h /= 2;
    
    for (row = 0; row < copy_h; ++row) {
        p_fg = fg_plane[1] + row * fg_pitch[1];
        p_bg = bg_plane[1] + row * bg_pitch[1];            
        p_alpha = alpha + row * 2 * alpha_pitch;  
        p_dst = mix_plane[1] + row * mix_pitch[1]; 
    
        for (col = 0; col < copy_w; ++col) {
            //*p_dst = ((bg) * (256 - alpha) + alpha * fg) >> 8;
            p_dst[col] = p_bg[col] + ((p_alpha[col << 1] * ((signed short)p_fg[col] - p_bg[col])) >> 8);
        }
    }

    // plane 2
    for (row = 0; row < copy_h; ++row) {
        p_fg = fg_plane[2] + row * fg_pitch[2];
        p_bg = bg_plane[2] + row * bg_pitch[2];            
        p_alpha = alpha + row * 2 * alpha_pitch;  
        p_dst = mix_plane[2] + row * mix_pitch[2]; 
    
        for (col = 0; col < copy_w; ++col) {
            //*p_dst = ((bg) * (256 - alpha) + alpha * fg) >> 8;
            p_dst[col] = p_bg[col] + ((p_alpha[col << 1] * ((signed short)p_fg[col] - p_bg[col])) >> 8);
        }
    }  
    
	return 0;
}

int HumanSegBase::bg_replace_rgb24(int width, int height, unsigned char *fg_buf, int fg_pitch, 
			unsigned char *bg_buf, int bg_pitch, unsigned char *mix_buf, int mix_pitch,
			unsigned char *alpha, int alpha_pitch)
{

	int copy_w;
	int copy_h;
	int row, col;
	unsigned char *p_fg, *p_bg, *p_alpha, *p_dst;
	
	copy_w = width * 3;
	copy_h = height;

	for (row = 0; row < copy_h; ++row) {
		p_fg = fg_buf + row * fg_pitch;
		p_bg = bg_buf + row * bg_pitch;
	
		p_alpha = alpha + row * alpha_pitch;  
		p_dst = mix_buf + row * mix_pitch; 

        //printf("p_alpha = %p, p_bg = %p, p_dst = %p, pitch = %d,%d,%d, row = %d, height = %d\n", p_alpha, p_bg, p_dst, alpha_pitch, bg_pitch, fg_pitch, row, height);
		for (col = 0; col < copy_w ; col += 3) {
			//*p_dst = ((bg) * (256 - alpha) + alpha * fg) >> 8;
			p_dst[0] = p_bg[0] + ((p_alpha[0] * ((signed short)p_fg[0] - p_bg[0])) >> 8);
			p_dst[1] = p_bg[1] + ((p_alpha[0] * ((signed short)p_fg[1] - p_bg[1])) >> 8);
			p_dst[2] = p_bg[2] + ((p_alpha[0] * ((signed short)p_fg[2] - p_bg[2])) >> 8);

			p_dst += 3;
			p_fg += 3;
			p_bg += 3;
			p_alpha++;

		}
	}

	return 0;
}

